
#    点云平面检测算法     https://zhuanlan.zhihu.com/p/15812577414
#                        https://www.sciencedirect.com/science/article/abs/pii/S0031320319304169

import open3d as o3d
import numpy as np

dataset = o3d.data.PCDPointCloud()
pcd = o3d.io.read_point_cloud(dataset.path)

# 用于检查点云是否含有法向量
#     assert 语句用于断言 当括号里的内容返回False时，程序直接报错终止
assert (pcd.has_normals())

# detect_planar_patches 方法   从点云中同时识别出多个独立的平面面片
#    参数 normal_variance_threshold_deg 控制点法向量之间允许的方差大小，值越小，往往会得到更少、质量更高的平面
#    参数 coplanarity_deg 控制点到平面的距离分布的允许范围，值越大，越能促使点在拟合平面周围的分布更紧密
#    参数 outlier_ratio 设置拟合平面相关点集中在被拒绝前允许的最大异常值比例
#    参数 min_plane_edge_length 用于拒绝假阳性结果 —— 平面面片的最大边长必须大于该值，才会被视为真正的平面面片。如果设为 0，算法会默认使用点云最大维度的 1%。
#    参数 min_num_points 决定相关八叉树的深度，以及尝试拟合平面时必须存在的点数。如果设为 0，算法会默认使用点云点数的 0.1% 作为该值
#    参数 search_param 是 geometry::KDTreeSearchParam 的实例，默认为 geometry::KDTreeSearchParamKNN    这是KDTree算法吧？
oboxes = pcd.detect_planar_patches(
    normal_variance_threshold_deg=60,
    coplanarity_deg=75,
    outlier_ratio=0.75,
    min_plane_edge_length=0,
    min_num_points=0,
    search_param=o3d.geometry.KDTreeSearchParamKNN(knn=30))

print("总共{} 个平面面片".format(len(oboxes)))

# 
geometries = []
# 不加这段代码的话生成的就是一个个框
# 遍历检测到的每个平面面片
for obox in oboxes:
    # 为平面创建一个极薄的三角形网格
    mesh = o3d.geometry.TriangleMesh.create_from_oriented_bounding_box(obox, scale=[1, 1, 0.01])
    # 用边界框自带的颜色给网格上色
    mesh.paint_uniform_color(obox.color) # 实际上发现网格上色与否对实际的显示都没有直接的影响 因为 obox 默认带 半透明填充色
    # 添加平面网格到可视化列表
    geometries.append(mesh)
    # 添加定向边界框到可视化列表
    geometries.append(obox)
# 添加原始点云到可视化列表
geometries.append(pcd)

# 可视化所有几何对象
o3d.visualization.draw_geometries(geometries,
                                  zoom=0.62,
                                  front=[0.4361, -0.2632, -0.8605],
                                  lookat=[2.4947, 1.7728, 1.5541],
                                  up=[-0.1726, -0.9630, 0.2071])
